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ABSTRACT 

This  thesis  presents  the  dynamic  modeling  and  its  application  to  the 
multi-link  robot  manipulators  with  joint  flexibility.  The  twist  angles  of 
springs  are  utilized  to  model  the  joint  flexibility.  A  direct  dynamic  model  is 
derived  by  the  Lagrangian  method  and  the  Newton-Euler  algorithm  is 
used  for  deriving  the  inverse  dynamics.  A  motion  controller  is  designed 
by  the  computed  torque  method  based  on  the  inverse  dynamics.  A 
recursive  estimator  is  included  in  the  control  design  to  estimate  the 
required  feedback  signals  which  are  not  directly  measurable. 


II! 


6.1 

TABLE  OF  CONTENTS 


I.  INTRODUCTION 1 

II.  NOTATION    AND    PRELIMINARIES 5 

A.  NOTATION  AND  PRELIMINARIES  FOR  THE  DIRECT 
DYNAMICS 5 

B.  NOTATION  AND  PRELIMINARIES  FOR  THE  INVERSE 
DYNAMICS 8 

III.  DIRECT  DYNAMICS 11 

IV.  INVERSE  DYNAMICS 20 

V.  CONTROLLER  AND  ESTIMATOR  DESIGN 28 

VI.  SUMMARY 32 

A.  CONCLUSIONS 32 

B.  RECOMMENDATIONS 33 

APPENDIX  A.     DERIVIATION  OF  THE  RESIDUAL  TERM 34 

APPENDIX   B.     DERIVATION  OF  DIRECT  DYNAMICS 37 

A.  THE  DYNAMIC  EQUATION  OF  LARGE  MOTION 38 

B.  THE  DYNAMIC  EQUATION  OF  SMALL  MOTION 46 

APPENDIX  C.     NEWTON-EULER  RECURSIVE 

FORMULATION 5 1 

LIST  OF  REFERENCES 54 

INITIAL   DISTRIBUTION    LIST 55 


IV 


LIST  OF  FIGURES 

Figure  1.  Error-Driven  Feedback  Control  System 4 

Figure  2.  Single-Link  Manipulator  With  Joint  Flexibility 12 

Figure  3.  Position  Vector  of  The  Center  of  Mass  of  Link  i 14 

Figure  4(a).  Free-Body  Diagram  of  Link  i 21 

Figure  4(b).  Resultant  Force  and  Torque  of  Link  i 22 

Figure  5.  Free-Body  Diagram  of  a  Link,  a  spring  and  a  motor.... 24 


ACKNOWLEDGMENTS 

I  would  like  to  thank  my  advisor  Professor  Liang-Wey  Chang  for  his 
unfailing  guidance,  full  support  and  patience.  He  provided  me  with  not 
only  the  technique  of  robotic  research  but  also  his  experiences  in 
programming.  His  open-door  policy  allowed  me  to  systematically  solve 
many  hard  problems. 

I  would  also  like  to  extend  deep  appreciation  to  my  family,  who  gave 
me  encouragement  and  total  support  throughout  the  entire  study. 


VI 


I.   INTRODUCTION 

The  problem  of  joint  flexibility  has  been  recognized  in  most  industrial 
robot  designs.  Joint  flexibility  typically  results  from  the  torsional  flexibility 
of  the  gear  mechanism  and  drive  shafts.  The  effect  of  joint  flexibility  is 
negligible  for  low-speed  and  light  payload  operations  and  the  rigid-body 
controllers  are  able  to  successfully  control  the  system.  The  rigid-body 
controllers  refer  to  controllers  that  are  designed  based  on  a  rigid-body 
dynamic  model.  As  the  speed  and  payload  increase,  the  inertia  forces 
and  gravity  forces  increase  such  that  the  flexibility  effect  becomes 
significant.  The  flexibility  may  not  be  tolerated  by  the  rigid-body  controller 
and  may  cause  system  instability.  The  improvement  of  the  dynamic 
behavior  requires  that  the  joint  flexibility  effects  be  incorporated  into  the 
controller  design  for  industrial  robots.  A  dynamic  model  of  industrial 
robots  with  joint  flexibility  is  therefore  needed. 

The  Lagrangian  method  was  applied  to  rigid-body  manipulators  to 
obtain  the  direct  dynamic  model  where  the  kinematics  was  described  by  a 
set  of  homogeneous  transformation  matrices  [Ref.  1:  pp.  1 58-1 72] .  The 
recursive  Newton-Euler  method  was  used  for  solution  of  the  inverse 
dynamic  response  to  compute  the  required  torques  for  rigid-body 
manipulators  [Ref.  2:  pp.  168-172].  Several  nonlinear  control  techniques, 
namely  feedback  linearization,  integral  manifold  and  composite  control 
[Ref.  3]  were  applied  to  design  controllers  for  a  single-link  manipulator 
with  joint  elasticity.  A  position  control  was  developed  for  a  two-degree- 
of-freedom  manipulator  where  the  effects  of  drive-train  compliance  and 


actuator  dynamics  were  included  [Ref.  4].  The  control  law  was  derived 
from  the  concepts  of  "computed  torque"  where  the  inverse  dynamics 
were  utilized  to  determine  the  required  torques  as  the  functions  of 
position,  velocity,  acceleration,  jerk  and  jerk  rate  errors.  With  the  same 
control  scheme,  the  computer  simulation  and  dynamic  analysis  of  a  single- 
joint  robot  was  conducted  [Ref.  5].  The  third  joint  of  the  PUMA  560 
robot  was  selected  as  an  example  and  a  comparison  between  the 
performances  of  rigid-body  controller  and  flexible-body  controller  was 
performed. 

In  this  thesis,  both  the  Lagrangian  dynamics  [Ref.  1]  and  the  Newton- 
Euler  algorithm  [Ref.  2]  will  be  applied  to  multi-link  robot  manipulators 
with  joint  flexibility.  The  former  is  used  to  derive  the  direct  dynamics  for 
the  plant,  computing  the  joint  motion  provided  that  the  torque  inputs  are 
given.  The  latter  is  used  to  develop  the  inverse  dynamics  which  returns 
the  required  torques  assuming  the  joint  motion  is  known.  This  thesis  also 
uses  the  method  of  computed  torque  [Ref.  4]  to  design  a  control  law  for 
multi-link  robot  manipulators  with  joint  flexibility.  This  control  approach 
presents  a  method  of  decoupling  the  arm  motion  when  drive-train 
compliance  and  actuator  dynamics  are  considered.  It  is  assumed  that  the 
angular  positions  and  velocities  of  both  links  and  motors  are  measurable 
from  shaft  encoders  and  tachometers.  Since  the  second  and  third 
derivatives  of  link  positions  (i,e.,  accelerations  and  jerks)  are  required  in 
the  control  design  and  not  directly  measurable,  an  estimator  is  designed  in 
a  recursive  manner  to  obtain  the  second  and  third  derivatives  of  link 
positions. 


To  present  the  frame  work  of  this  thesis,  Figure  1  shows  an  error- 
driven  feedback  control  system  which  includes  the  inverse  dynamics 
(controller),  direct  dynamics  (plant)  and  the  estimator.  In  this  figure,  the 
system  is  designed  to  follow  a  reference  input  and  the  system  error  is 
generated  by  the  difference  between  the  desired  reference  input  and  the 
feedback.  The  Newton-Euler  method  is  used  to  design  the  controller 
since  its  recursive  formulation  allows  for  the  efficient  computation  of  the 
inverse  dynamics  and  therefore  possibly  for  on-line  control.  The 
Lagrangian  method  derives  the  direct  dynamics  and  models  the  plant  for 
computer  simulation  because  of  its  systematic  nature  which  is  convenient 
for  programming. 

The  objective  of  this  thesis  is  to  formulate  the  direct  and  inverse 
dynamics  of  multi-link  robot  manipulators  with  joint  flexibility.  In  addition, 
a  computed-torque  control  is  designed  to  show  an  application  of  the 
inverse  dynamics.  Notations  and  preliminaries  will  be  presented  in 
Chapter  II.  The  direct  dynamics  and  the  inverse  dynamics  will  be 
discussed  in  Chapter  III  and  Chapter  IV,  respectively.  Finally,  the 
controller  and  estimator  design  procedures  will  be  developed  in  Chapter 
V. 
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II.   NOTATION   AND   PRELIMINARIES 

This  section  defines  the  notations  used  in  the  thesis  and  briefly 
discusses  some  of  the  basic  fundamentals  which  will  be  used  to  develop 
the  model. 

A.  NOTATION  AND  PRELIMINARIES  FOR  THE  DIRECT 
DYNAMICS 

at  :  one  of  the  four  Hartenberg-Denavit  parameters  representing  the 
distance  between  two  adjacent  frames  j-1  and  /  in  x  direction. 

ai  :  one  of  the  four  Hartenberg-Denavit  parameters  representing  the 
rotation  angle  about  the  x  axis. 

d,  :  one  of  the  four  Hartenberg-Denavit  parameters  representing  the 
distance  between  links  in  z  direction. 

6,  :  one  of  the  four  Hartenberg-Denavit  parameters  representing  the 

rotation  angle  of  links  about  the  z  axis, 
q.  :  joint  variable  of  link  i.  For  prismatic  joints  /,  q,  is  dit  for  revolute 

joint/,  q,  is  0,. 
qmi  :  the  rotation  angle  of  rotor  i  about  the  z  axis. 

G(  :  gear  ratio,  i.e., 

^       rotation  angle  of  i  -  th  motor  shaft  n  n 

O  = v^-l) 

rotation  angle  of  i  -  th  link 

A(    :  the  Hartenberg-Denavit  matrix  for  link  /,  i.e., 


A  = 


10  0                   0 

fl.cosq.     cosq,  -sinq.cosa  sinq.sina 

<3.sinq.     sinq.  cosq.cosa,  -cosq.sincr 

d:           0  sina              cos  a 


(2.2) 


Am.  :  homogeneous  (4x4)  coordinate  transformation  matrix  of  motor 


i, 


i.e., 


where 


A  ,= 


10            0  0 

0    cosq^  -sinqm  0 

0     sinq  cosq  0 

0        0            0  1 


(2.3) 


q-=G1ql-5l 


(2.4) 


W,  :  homogeneous  (4x4)  coordinate  transformation  matrix  from  the  i- 

th  coordinate  frame  to  the  base  coordinate  frame. 
W/  :  homogeneous  (4x4)  coordinate  transformation  matrix  from  the 

z-th  coordinate  frame  to  the  7-th  coordinate  frame. 
mf  :  mass  of  the  /-th  link. 
mm.  :  mass  of  the  /-th  motor. 
r'  :  local  position  vector  of  the  center  of  mass  of  link  i  decomposed  in 

the  body-fixed   coordinate. 


r.:   absolute   position   vector  of  the   center  of  mass   of  link   i 

decomposed  in  the  base  coordinate. 
rmi:  absolute  position  vector  of  the  center  of  mass  of  motor  i 

decomposed  in  the  base  coordinate. 
r'm,  :  local  position  vector  of  center  of  mass  of  motor  j  decomposed 

in  the  motor  body-  fixed  coordinate. 
J    :  the  pseudo  inertia  matrix  of  the  z'-th  motor 

JL=Jiir!!diii 

rru  J      mi     mi 


0 

0 

mizz 

2 

0 

0 

0 

0 

I 

mizz 

2 

0 

0 

0 

0 

I 

mi/./ 

0 

0 

0 

2  J 


(2.5) 


where  I      is  the  moment  of  inertia  of  motor  i  in  the  local  z  axis. 

BUZZ 

Note:  The  elements  in  the  first  row  and  the  first  column  are  zero 
since  the  mass  m    of  the  motor  i  is  considered  to  be  included  in 

mi 

the  link  i. 

the  pseudo  inertia  matrix  of  the  i-xh  link,  where 
J  =Jr'r,Tdm 
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m  x 


m.x 


m  z 

i  i 


mixi 

i   A\\ 

iax  iyy  ui 


ay 


m  .y. 

i  -i  »i 

ixx  iyy  iu 


lyi 


m  z 


ixii  iyy  ~     izz 


(2.6) 


where  m(x.,  m(y(,  m(z(  are  the  first  moments  of  link  i,  1^,  Iiyy,  1^ 
are  the  moments  of  the  inertia  of  link  i  in  the  principal  axes  and 
1^,  1^  ,Iiyz  are  the  products  of  inertia  of  link  i. 

B.  NOTATION  AND  PRELIMINARIES  FOR  INVERSE 
DYNAMICS 

coi  :  absolute  angular  velocity  of  link  i  m 

d)i  :  absolute  angular  acceleration  of  link  / 

(bi  :  change  rate  of  d)r 

coi  :  change  rate  of  cor 

Qi  :  absolute  angular  velocity  of  link  i  in  a  matrix  form. 

0)u  :  local  relative  angular  velocity  of  link  i  decomposed  in  coordinate 

M,i.e.,[0    0    q,]T 
cbu   :  local  relative  angular  acceleration  of  link  i  decomposed   in 

coordinate /-l,  i.e.,  [0    0    qj 


a>u  :  local  relative  angular  jerk  of  link  i  decomposed  in  coordinate  i- 1 , 

U.,[0    0    q.f 
a,  :  linear  acceleration  of  link  i 

^mi  :  absolute  angular  velocity  of  motor  i  decomposed  in  coordinate  i- 

1. 
fi>m  :  change  rate  of  com. 

aa  :  linear  acceleration  of  link  i  at  the  center  of  mass. 
F  :  resultant  force  exerted  on  link  i  at  the  center  of  mass. 
f . :  force  exerted  on  link  i  by  link  i  - 1 . 

N.:  resultant  moment  exerted  on  link  i  about  the  center  of  mass. 

n.  :  moment  exerted  on  link  i  by  link  i-l. 

nm  :  moment  exerted  on  motor  i  decomposed  in  coordinate  i. 

T  :  torque  generated  from  the  electromagnetic  field  of  motor  i  in  the 

local  z  axis. 
R;  :  rotational  transformation  (3x3)  matrix  from  the  7-th  coordinate 

frame  to  the  i-th  coordinate. 
K(>  :  control  gain. 

ki  :  spring  stiffness. 

k  :  unit  vector  in  the  local  z  axis. 

I.  :  inertia  tensor  of  link  /,  i.e., 


I .  = 


III 

K 

I 

I 

lxy 

1 

iyy 

I 

I,„ 

I 

lyz 

I 

lyi 


(2.7) 


I    :  moments  of  inertia  of  motor  z,  i.e., 


I.  = 


I  0        0 

nuxx 

0      1  0 

miyy 

0        0      I  s 


(2.8) 
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III.  DIRECT  DYNAMICS 

The  Lagrangian  dynamics  approach  is  used  to  derive  direct  dynamics 
for  the  equation  of  motion  of  the  plant  in  terms  of  the  generalized 
coordinates.   The   Lagrangian  dynamic  equations  are  given  as  : 


_d 
dt 


f 


V 


<?(KE) 
9  i  . 


^3^  =  P,    0=1,2 n)  (3.1) 

where  §y  is  the  generalized  coordinate,  n  is  the  number  of  degrees  of 
freedom  and     P>  denotes  the  generalized  force. 

This  thesis  considers  a  plant  of  a  six-link  manipulator  with  revolute 
joints  actuated  by  DC-motors.  Each  motor  is  connected  through  a 
transmission  shaft  (Figure  2)  to  a  rigid  link  and  a  spring  with  a  constant 
stiffness  is  modeled  to  be  a  major  source  of  joint  flexibility.  The  twist 
angle  of  the  spring  §.  is  defined  as  St  =  G.q,  -qmi  where  G,  is  the  gear 

ratio.    Since  additional  degrees  of  freedom  are  introduced  by  the  twisted 
spring  ,  the  generalized  coordinates  are  defined  as: 

§=[qi.qa.»»q«AA"»£i] 

The  generalized  force  P;  is  obtained  from  the  concept  of  virtual  work. 
Since 

8W  =  iT8qm=tr(G,Sq,-8  8,) 


11 


J  m" 


m 


FIGURE  2.  Single-Link  Manipulator  With  Joint  Flexibility 
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therefore,  TG.  is  the  generalized  force  corresponding  to  the  generalized 
coordinate  q.,  and  -T  is  the  generalized  force  corresponding  to  the 
generalized  coordinate  8.. 

The  total  kinetic  energy  of  the  system  consists  of  the  individual  kinetic 
energies  of  the  links  and  motors  and  the  total  potential  energy  is 
composed  of  the  strain  energy  due  to  joint  elasticity  and  the  potential 
energy  due  to  gravity.  The  kinetic  energy  KE  and  the  potential  energy  PE 
are  derived  as  follows:  Given  a  point  specified  by  a  local  position  vector 
r'  (Figure  3),  its  position  and  velocity  with  respect  to  base  coordinates  are 
given  as 

r  =Wr'  (3.2) 

tit  v      ' 

and 

r.  =W.r*  (3.3) 

The  kinetic  energy  of  link  /  is 

(KE)uc,=ItrJf,rTdm  (3.4) 

For  six-link  robot  manipulators,  the  total  kinetic  energy  becomes 

(KE)u[=t(KE)u[i 

i'=l 

=  i^rJrrTdm  (3.5) 

i=i  2 

Substituting  Equation  (3.3)  into  Equation  (3.5)  gives 

(KEL  =  t^tr[w,(Jr,'r;Tdra)W,]  (3.6) 

i=l    ^ 
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z, 


coordinate  i 


Figure  3.  Position  Vector  of  The  Center  of  Mass  of  Link  i 
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The  integral  in  Equation  (3.6)  is  known  as  the  pseudo    inertia   matrix    J, 
and  was  given  in  section  II.  Equation  (3.6)  thus  becomes 


(KE)ut=t^r[WiJiWi] 


~2 
Similarly,  the  kinetic  energy  of  the  motors  is 

(KE)    -y-trfr  f  Tdm 

V  /m  L~i  ,-»        J     mi    m« 


where 


r    =W    A   r 

mi  «-l        mi    mi 


The  time  derivative  of  r    is  obtained  as 


r    =W    A   r    +W    A   r 

mi  i-l        mi    mi  i-l        mi    mi 


and 


A_ .  = 


1 


0 


0 


0' 


0    cosqmi     -sinqmi     0 
0     sinqmi      cosqmi      0 

±  mi  T  m* 


0        0  0  1 

Substituting  Equation  (3.10)  into  Equation  (3.8)  gives 


(KE)m=X±tr 

i=l    L 


(W  ,A   r'.+W.  .A.r') 

\         i-l        mi    mi  i-l        mi    mi  / 


•(W,A   r'+W,  .At')7  ldm 

\        i-l       mi    mi  i-l       mi    mi  / 

=  -trf\V  ,A    (fr'T'Tdm)A^W(T1 

n.         I  »-l        mi  \J      mi     nu  /        mi  i-l 

+W  ,A    (fr'r'Tdm)AT  WT, 

i-l        mi  y  J      mi    mi  /        mi         i-l 


(3.7) 


(3.8) 


(3.9) 


(3.10) 


(3.11) 
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+W..A  .(fr'.r,T.dm)AT.W.T1 

»-l        nuy      mi    mi  J       mi         i-l 

+W(  ,A  Vfr^dmU^.W;1, 

*— 1        nu  \J      nu    nu  I        mi         i— 1 


Since 


tr 


%  1Am(fri.r£.dm)A^.WiT1l  =  trfw  ^.(fr^.dmW.W*, 

i—l        nu  \  J      nu    mi  t       mi         I— 1   I  i— 1        mi  \  J      nu    mi  I        nu         I—  1 

Equation  (3.12)  becomes 

(KEL  =  S-trfW,.  AJ^lW'i 

\  '  m  ^^  /■»  i— 1       mi     mi       mi        «— 1 


£T2 


where 


+2W.  .A  J  .AT.WT, 

i—l        mi     mi        mi         i— 1 

+W  ,A   J   AT.WT,1 

i—l        mi     mi        mi         i— 1  J 


J    =[r'r,Tdm 

m/        J      mi    mi 


and  the  total  kinetic  energy  of  the  system  can  be  found  as 

(KE)  =  (KB)„+(KE). 

The  potential  energies  of  links  and  motors  are  obtained  as 

(ra)ix=-imrfg 


i=l 

6 


=  -£mr:TW,Tg 


i=l 


and 


(PE), 


-Ym   rT  g 

i_^  mi     cm  i  ~ 

i=l 

-Ym    (W ..A   r'   )Tg 

^_^         mi  V         i-l        mi    cmi  /      o 
i=l 

-Ym  .r,T.AT.W.T]g 

,^_<         mi    cmi        mi         HO 


(3.12) 


(3.13) 


(3.14) 


(3.15) 


(3.16) 


(3.17) 


(3.18) 


1=1 


where 


r  o  l 


g  = 


0 

0 

-9.8 


m 


VseC 


The  center  of  mass  of  link  /  is  described  by  a  local  vector  r^  and  the 
center  of  mass  of  motor  i  is  described  by  a  local  vector  rV.  Their  position 
transformations  from  local  coordinate  to  base  coordinate  are  defined  as 
rc,  =  W^  and  rOTi  =  W^A^rV,  respectively. 

The  potential  energy  due  to  strain  is 


(pe),  =  ^£mv 


(3.19) 


Therefore,   the   total  potential  energy   of  the   system   is   obtained  by 
combining  Equations  (3.17),  (3.18)  and  (3.19)  : 

(PE)  =  -ymr,TWTg-Ym  r'l A* W\g  +  -Y k A2     (3.20) 

V  jL-,  i     ci  i     B  X—t         mi     cm i         mi         i-lO  ->    ^L-'        i      i  v  ' 

Z   1=1 


1  =  1 


i=l 


Since  generalized  coordinate  q.  is  for  large  motion  and  generalized 
coordinate  8t  is  for  small  motion,  two  sets  of  equation  of  motion  will  be 
obtained  according  to  Equation  (3.1).  For  the  large  motion,  the  general 
form  of  dynamic  equation  is  described  as 

Ytr(W  JWTWtr(X.J   AT    W\)+  YtrfX.J  ALW.M 

Z_w       \        i.j     i        t     )  \       j     m;       ny.y         j-\  )  jL-       \       j     my       ny         i-l,;  / 


'=; 


<=;+l 
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b  b 

-2>irr Wyg - mny.r^A^..W/_1g -  Xm^A^-W^g  =  G/I\ 

(3.21) 


i=y  i=/+l 


where 


X,=W,.,Am,  (3-22) 

*-,=*hK+*mK  (3-23) 

X,=W,  ,A  . +2W.  .AW.A  .  (3.24) 

V  y-l       ny  y-1       ny         y-1       ny 

The  dynamic  equation  for  the  small  motion  is  described  as 

-trfX.J  .AT..W.T,)  +  m  r''TAT.  W.T,g  +  k  8 .  =  -T.  (3.25) 

\       j     mj        ny./         i-l  /  my    ny         ny,;         j-\o  j      j  j  \ 

The  second  time  derivative  of  the  transformation  matrix  W  can  be 
derived  as 

W,   =iw,4ql+iiw,t.q,qm  (3.26) 

k=\  k=\  m=\ 

where  XXW1Jtmqtqm  is  defined  as  the  residual  term  that  includes  Coriolis 

k  =  \  m=\ 

and  centripetal  accelerations.  A  recursive  form  (see  Appendix  A)  is  used 
to  compute  the  residual  term  to  avoid  tedious  computation.  The  detailed 
procedure  of  direct  dynamic  equations  is  derived  in  Appendix  B. 

For  the  special  case  of  a  rigid  body  plant,  the  stiffness  of  the  spring 
approaches  infinity.  Therefore,  the  elastic  displacement  is  equal  to  zero, 
and  Equation  (3.21)  for  the  rigid  body  case  reduces  to  : 

t  tr(W,,J,W/  )  -  Im,r;' W,;;g  =  T  (3.27) 


»=/  '=/ 
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In  this  case,  the  Equation  (3.25)  presenting  the  small  motion  can  be 
ignored  directly. 
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IV.     INVERSE   DYNAMICS 

The  inverse  dynamics  is  to  predict  either  the  required  driving  force  or 
torque  for  a  mechanical  system  of  which  the  motion  (i.e.,  the  joint 
positions,  velocities,  accelerations  and  jerks  in  this  thesis)  is  known.  The 
inverse  dynamics  of  robot  manipulators  with  joint  flexibility  is  developed  in 
this  section  by  the  Newton-Euler  method.  The  controller  and  the 
estimator  are  designed  based  on  the  inverse  dynamics  for  that  is 
computationally  efficient  and  has  potential  for  on-line  control. 

Figure  4(a)  presents  a  free  body  diagram  of  the  link  /  where  f  and  ni 
are  force  and  torque  exerted  by  link  i-\  decomposed  in  coordinate  i  and 
f+1and  ni+,  are  force  and  torque  exerted  by  link  i+\    decomposed   in 

coordinate  i  +1.  This  system  of  forces  and  moments  is  equivalent  to  that 
on  Figure  4(b)  where  F  and  Nt.  are  the  resultant  force  and  torque  acting 
on  the  center  of  mass.  Therefore,  the  force  and  moment  balance 
equations  are  obtained  as 

F=f  -Ri+1f  ,  (4.1) 


i+i 


N.  =n.  -R,+1n.+1  -SI  xf  -CI  xF  (4.2) 

III  l+I  fill  N  t 


i  +  l 


The  Newton-Euler  equations  can  then  be  written  as 

F=m(ac,  (4.3) 

Nf  =1^+0,(1^)  (4.4) 

By  taking  balance  of  moments  in  the  local  z  direction  on  the  free  body 

diagram  of  the  motor  /  (the  bottom  figure  of  Figure  5),  the  torque  equation 

can  be  expressed  as 
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coord,  i 


(a) 


Figure  4(a).  Free-Body  Diagram  of  Link  i 
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(b) 


Figure  4(b).  Resultant  Force  and  Torque  of  Link/ 
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T.  =(R'  n  )k  +  (l   cb    +Q  ,1   co   )k 

i  V       i—l      i  /  \    mi       mi  i-l    mi       mi  / 


(4.5) 


where 


CO     =G),+< 

mi  i-l 


0 
0 


(4.6) 


and 


co  -  =  co  .  .  +i 

mi  i-l 


0 
0 


1<L 


(4.7) 


From  Equations  (4.5),  (4.6)  and  (4.7),  the  required  torque  T  can  be 
obtained  if  the  kinematics  of  the  manipulator  is  given.  The  recursive 
inverse  dynamics  of  rigid-body  systems  has  been  available  [Ref.  6]  where 
the  gravity  has  been  incorporated  into  the  kinematics.  For  flexible-joint 
systems,  the  motor  variables  are  not  identical  to  the  link  variables  and  qmi 
and  qmi  will  be  related  to  link  variables  in  this  research. 

Figure  5  shows  three  free-body  diagrams,  i.e.,  free-body  diagram  of 
link  /,  the  i-th  spring  and  motor  /,  which  illustrate  the  relations  among  the 
moment  ir,  the  spring  torque  k(5(  and  the  torque  T.  produced  by  the 
electromagnetic  field  of  the  motor.  Since  only  the  moments  in  local  z  axis 
are  concerned,  the  moments  acting  on  the  both  sides  of  the  spring  along  z 
axis  are  equivalent  in  magnitude  and  opposite  in  directions.  In  other 
words,  the  component  of  the  moment  rr  in  the  local  z  axis  acting  on  link  i, 
i.e.,  (R]._,n.)-k,  is  equal  to  that  acting  on  the  spring  in  the  local  z  axis. 
This  relation  can  be  given  as 
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Link/ 


icy., 


-kl: 


k,& 


>\[\M\fV— (R;-,n,)k 


k,5,=k,(G,q,-qm)  =  (R:.1n,)k 


k<5. 


^    mi       mi  i—  l     mi        mi  / 


Figure  5.  Free-Body  diagram  of  a  link,  a  spring  and  a  motor 
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M>,=(R;.,n,)-k  (4.8) 


or 


5,=-J-(R;_,n,)k  (4.9) 

i 

As  we  defined  8.  in  section  II,  qm  can  be  described  as 

Qn^G^-S,  (410) 

By  substituting  Equation  (4.9)  into  Equation  (4.10),  the  angular  position  of 
motor/,  i.e.,  qm,,  can  be  expressed  as  a  function  of  generalized  coordinate 

q,: 

q     =Gq  -—  (R!  ,n  )-k 

Inn  i  Ti  l        V.        i-I       <  / 

I 

The  above  equation  is  equivalent  to  the  following  equation  where  the 
vectors  are  decomposed  in  the  base  coordinates. 

qm,=G,q,--J-(R;n,)-(R;-k)  (4.14) 

As  referring  to  Equation  (4.7),  the  first  and  second  time  derivatives  of 
angular  position  of  motor  i  are  required  to  compute  the  torque  T.  By 
applying  RJ  =  RJQ;,  qmi  and  qm,  can  be  obtained  as 

q*  =GA ,~  [RtfO^  +h,)(Rrk)  +  (R>,)(Rrn,.1k)]    (4.15) 

q- ■  =  G,q,  -jJ-{r;("P,",  +  ",n,  +  2ii,n,  +  n.)  •  (r;1  k) 

i 

+2r;  (fi.n,  +  n, )  •  (r;- ",-k) + (r>,  )  ■  [r;-  (n_,k  +  a,.,a,_,k)]} 
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where 


n  =  N  +R*:+1n.+1  +S1  xf  +  C1  xF 


(4.16) 


(4.17) 


ri.=Q.N.+N.+R':+1(Q.+1n.+1+n.+1)  +  Q  (SI  xf  )  +  Sl  xf 

i  «       i  i  i      \       i+l     i+l  i+l  /  i  V,       i  i  /  i  i 

+  Qi.(Cl.xF.)+ClixF.-fl.n.  (4.18) 

ft,  =OAN|  +2Q.N,  +Q.N,  +N,  +R;+1(Q1+In,+In.+1 
+  2Q,+1ri, +I  +  Q,+1n,+1  +  hi+l )  +  O  ,Q,  (SI,  x  f. ) 

+  2H  (Sl,xf  )  +  Q(SI,xfJ  +  Sl(xf. +aa(Cl.xF.) 

+  2Q  (CI  xF.)  +  Q  (CI  .  xF.WCl  xF  -QQn 
i\i       i/        «\'       •/         •       <        iii 

-2Q,ri,-Q,n,  (4.19) 

N,  =I,<y  +Q,(l,<y,)  (4.20) 

N  =  lib.  +  Q.1Q)  +Q.I  <b  (4.21) 

i       i    i        i  i    i        i  i    i  v         ' 

N  =1  iii  +nico  +2Qlti)  +Q  I  0)  (4.22) 

i       i    i        i  i    i  i  i    i        i  i    i 

co^R'-^co^+co,,)  (4.23) 

d),=R;-I(a,_1o;/i,+co,_1+<.)  (4.24) 

+CQIi)-QiQ)i  (4.25) 

a),.  =  R;1(ai.1a,1aj._1a)/.  +oHoH<&H  +3a  1q,_]< 

+  2Q,_1Q,_1^,  +2Q,_1a),_1  +3Q,_16)/„  +n,_1Q,_1a)/„ 


+n,_1a),_1  +3QHffllil  +«,._,  a),.  +  ®M  +  aJw) 


(n  ft  d) .  +  2Q  d)  .  +  Q  <y  ) 

\    i    i    i  ii        i    i  j 


(4.26) 
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The  intermediate  variables  f.,  F(,  and  their  rates  of  change  are  described 
in  Appendix  C. 
Since 


®u  =  < 


0 


0 

lAJ 


differentiating  Equation  (4.27)  with  respect  to  time  gives 


(4.27) 


(T 

(T 

(T 

<  =  < 

0 

>  fiw  =  < 

0 

>  Q)lt  =  < 

0   > 

A, 

A . 

qIV 

(4.28) 


Substituting  Equations  (4.27-28)  into  Equation  (4.5)  yields  a  fourth- 
order  ordinary  differential  equation.  The  inverse  dynamics  can  be  simply 
describes  as 

T  =j;q|v+f  (4.29) 

where  f  is  a  nonlinear  function  of  arm  position,  velocity,  acceleration  and 
jerk,  the  term  J*  is  a  function  of  the  moments  of  inertia  of  motors  and  links 
and  spring  constants.   T  is  the  required  torque. 
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IV.  CONTROLLER  AND  ESTIMATOR  DESIGN 

The  purpose  of  designing  an  controller  for  the  manipulator  is  to  drive 
the  tracking  error  to  zero  asymptotically.  In  this  section,  a  method  of 
computed  torque  [Ref.  4]  is  utilized  as  a  control  scheme  to  determine  the 
required  torques. 

Define  the  error  as 

£  =  qlD-q,  (5.i) 

where  q,D  is  the  desired  angular  position  of  link  i  and  q,  is  the  measured 
angular  position  of  link  i.  The  error  dynamics  for  the  fourth-order  system 
(Equation  (4.29))  is  described  as: 

£rv+K.3£+K2£  +  K,£  +  K0£  =  0  (5.2) 

where  the  K's  are  constants  representing  the  feedback  gains.  By 
substituting  q[v  in   Equation   (4.29)   to   the   error  Equation   (5.2),   the 

computed  torque  is  then  obtained  as 


where  J*    is  a  calculated  value  of  J*  and  f  .  is  the  calculated  value  of   f . 

ical  i  teal  i 

q^  is  the  "calculated  jerk  rate"  given  by 

<\l=^Z  +  K{^-l)  +  K2{^-i)  +  K{^-i)  +  Ko{^-^)       (5-4) 

where  q^Q.'CJ,  and  q.  are  measured  variables.  J'^and  ftcal  can  be 
computed  from  Equation  (4.29)  based  on  the  complete  knowledge  of  the 
plant  dynamics  and  availability  of  measurements. 

ITAE  performance  criteria  [Ref.  7]  for  a  fourth-order  equation  is  used 
for  the  selection  of  the  k  values.   The  values  are 
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k,  =  2.lco 


ki2=3Aco2a 

kn=  2.1  col 

ki0=1.0<un4  (5-5) 

where  con  is  a  selected  value  for  the  servo  bandwidth. 

For  a  rigid-body  case,  the  stiffnesses  of  all  springs  approach  infinity, 

and  the  St  approaches  zero.    Equation  (5.3)  is  reduced  to  a  second-order 

ordinary  differential  equation  : 

T,  ^Lq^+Li  (5-6) 

where 

q.cai=q1B  +  k11(^.B-q1)+k,o(q1D-q1)  (5.7) 

and  the  values  of  the  feedback  gains  according  to  the  ITAE  are  : 

kn=1.4a>„  (5.9) 

ki0=1.0<»n2  (5.10) 

The  fourth-order  system  requires  four  measurements  for  each  link  to 
compute  the  driving  torques.  It  was  assumed  that  the  angular  positions 
and  velocities  of  both  links  and  motors  are  available  from  shaft  encoders 
and  tachometers.  The  second  and  third  derivatives  of  link  positions  (i,e., 
accelerations  and  jerks)  are  not  measurable.  Equation  (5.4)  shows  that 
the  position,  velocity,  acceleration  and  jerk  of  link  are  required  to  compute 
the  torques  to  drive  the  plant.  An  estimator  in  a  recursive  form  is 
designed  to  estimate  those  variables  that  are  not  measurable  (i.,  q.  and  qV 
)  by  using  the  measurements  from  those  variables  that  are  accessible  (i.e., 
q.4,»qmi  and  qj. 

By  substituting  Equation  (4.20)  into  Equation  (4.17),  it  becomes 

n,  =  1,6),  +0,(l,ft>,)  +  R;*'n,H  +SI,  xf,  +C1,  xF,  (5.1 1) 
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then  substituting  Equation  (4.24)  into  Equation  (5.11)  gives 

n, ^  =I<R;-1(Q.  xa>u ,+©,..,  +  <)  +  01.(l(<tf1)  +  R;+1n 
+  S1  xf  +C1  xF 


(5.12) 


Substituting  Equation  (5.12)  into  Equation  (4.8)  yields 

M,  ={rL![iAm(Om»m  +  *«  +  ^)+Oift®*) 

+R;+1n,+1+Slixf,+Cl,xFi.]}.k  (5.13) 

Equation  (5.13)  is  rearranged  as 

(RLllRr*u)-k  =  Mi-{RU[i|Rr(OM%  +  *M)  +  0,(i,®i) 

-R;:>.+1+Slixfi+Cl1.xFi]}-k  (5.14) 

Recall  Equation  (4.28)  where 


®i..-  = 


0 
0 

A  J 


therefore  Equation  (5.14)  can  be  rewritten  as 

V      »-l    '      '      /33 

-R;:+1ni+1+sil.xf;.+a,.x^]}.k)  (5 15) 

where  (Rj^I-Rj-1)     represents  the  element  of  the  third  column  and  the 
third  row  in  matrix  (R^I.R)."1 )  . 

Again,  Differentiating  Equation  (4.8)  gives 

ki^(R;.Ani+R;.1h1)-k  +  (R;.1n,).QHk  (5.16) 

Similarly,  referring  to  Equations  (4.17),  (4.18),  (4.21),  (4.25)and  (4.28), 
Equation  (5.16)  becomes 
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q.  =-r~. — -(k  5  -|R':  .Q n  +R!  ,[2*2.1.©.  +QQI  co 

IB'    T U      1     V  '  iii  i     i  i     i 

V       i-l    I       i      /33 

+6,1,0). ,-q.n,  +  i,R;-'(Qi_1n,_1ft),„  +ii,_1<j)._1  +2n,.,d),, 

+6,_1a),,  +6>,.,)  +  Q,(Sl,  xf,)  +  Sl,  xf,  +n,(Cl,  xF.) 

+C1,  xF,]}-k)-  l  (R;-,n,)n,k  (4.48) 

V       i-l     i       i      /33 


where 


8 =Gq -q 

i  ill  Imi 

For  the  rigid-body  controller  case,  as  8i  approaches  zero,  the 
number  of  degree  of  freedom  of  link  i  reduces  to  one.  The  system 
governing  equation  then  becomes  a  second-order  ordinary  differential 
equation.  Since  the  required  feedbacks  qi  and  q,  are  measurable  ,  no 
estimation  is  needed. 
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VI.  SUMMARY 

A.    CONCLUSIONS 

A  direct  dynamic  model  and  an  inverse  dynamic  model  have  been 
developed  for  multi-link  robot  manipulators  with  joint  flexibility.  Since  an 
additional  degree  of  freedom  was  considered  for  joint  flexibility,  two  sets 
of  system  dynamic  equations  were  derived  by  the  Lagrangian  method  for 
direct  dynamics  which  returns  the  robot  joint  motions  according  to  input 
torques.  The  first  equation  described  the  large  motions  of  links  while  the 
second  described  the  small  motions  of  twist  angles  of  joint  springs.  These 
two  equations  are  expressed  in  a  form  suited  for  the  sequential  integration 
method  [Ref.  8].  Moreover,  the  Lagrangian  method  is  systematic  and 
easy  to  program. 

The  Newton-Euler  method  was  used  to  develop  the  inverse  dynamics 
which  returns  the  required  torques  assuming  the  robot  joint  motions  are 
known.  The  Newton-Euler  method  provides  efficient  computation  and 
has  potential  for  the  on-line  control  purpose.  The  inverse  dynamics  for 
multi-link  robot  manipulators  was  derived  by  decoupling  the  link  dynamics 
and  motor  dynamics.  A  fourth-order  system  equation  of  link  variables 
was  obtained  and  a  fourth-order  error  dynamics  was  designed  for 
controller.  This  control  was  finally  achieved  by  the  design  of  a  recursive 
estimator  which  estimates  the  second  and  third  time  derivatives  of  link 
positions.  The  advantage  of  the  estimator  is  to  eliminate  the  differentiation 
process  that  would  cause  noise  problems. 
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B.    RECOMMENDATIONS 

The  recommendations  are  listed  as  follows: 

1.  Continue  to  perform  computer  simulation  for    multi-link  robot 
manipulators  with  joint  flexibility. 

2.  Evaluate  the  performance  of  flexible-body  controller. 

3.  Implement  the  flexible-body  controller  for  on-line  control. 
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APPENDIX  A.  DERIVIATION  OF  THE  RESIDUAL  TERM 

The  residual  acceleration  mentioned  in  this  thesis  includes  all  the 
nonlinear  terms  caused  by  the  Coriolis  acceleration  and  centripetal 
acceleration  due  to  link  motion.  The  residual  accelerations  can  be 
computed  through  a  recursive  form  and  computational  efficiency  can  be 
therefore  improved  since  the  detailed  computational  procedure  of  each 
nonlinear  term  is  skipped. 

Recall  Equation  (3.26): 

^,  =ttw^4.4t+twuq,  (A.i) 

4=1  m=l  *=1 

The  residual  acceleration  is  defined  as 

**=t£w»AA  (A.2) 

*=1  m=\ 

The  recursive  form  Wn  is  derived  as  follows.  The  derivation  begins 
with  the  recursive  relation  of  homogeneous  transformation  matrix  W., 
W  and  W  . 

W,  =  W^A,  (A.3) 


Therefore, 


W.  =W_1A.+W_1A.  (A.4) 


since 


W  =  W  ,A  +2W.  ,A.  +  W.  .A.  (A. 5) 

<         i-i    ■  i-i    i         i-i    i  x 


A^QAA+QAA  (A.7) 
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where 


Q  = 


(A. 8) 


0  0     0     0' 

0  0-10 

0  10     0 

0  0     0     0 

is  called  the  operative  matrix  for  revolute  joints.    For  prismatic  joints,  it 
becomes 


Q  = 


"0000 
0  0  0  0 
0  0  0  0 
10    0    0 

The  recursive  form  of  W  is  rewritten  as 

W   =Wi_1Ai  +Wi.1Ai+W,1QAl4+W,._1QAI.qi.+W,1QAj.qi 


(A. 9) 


Iff  i=l, 


where 


W  = 


1 

0 

0 

0" 

0 

1 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

then 


(A.10) 


W,  =W0A1+W0A1+W0QA1qI+W0QA1q1+W0QA1q1  (A.ll) 


(A. 12) 
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W0=W0  =  0  (A.13) 

and  (A.ll)  becomes 

W,  =  W0QA1q1+W0QA1q1 

=  W0QQA1q1q1+W0QA1q1  (A.  14) 

According  to  Equation  (A.l),  Equation  (A.  14)  can  be  expressed  as 

W,  =Wrl+W0QA1q1  (A.  15) 

where 

W„  =  W.QQA.q.q,  (A.  16) 

Ifi'=2, 

W2  =  W,A2  +  W,A2  +  W,QA2q2  +  W,QA2q2  +  W,QA2q2 

=  W,A2  +  W,QA2q2  +  W,QA2q2  +  W,QQA2q2q2  +  W,QA2q2 
=  W,A2  +2W,QA2q2  +  W,QQA2q2q2  +  W,QA2q2       (A.  17) 
Substituting  equation  (A.  15)  into  (A.  17)  yields 

W2  =(W„  +W0QAiq,)A2  +2W,QA2q2  +  W,QQA2q2q2  +  W,QA2q2 

=  (WtlA2  +2W,QA2q2  +  W,QQA2q2q2)  +  W0QA,A2q, 
+W,QA2q2  (A18) 

where 

Wt2  =  W„  A2  +  2W,QA2q2  +  W,QQA2q2q2  (A.  1 9) 

Similarly,  the  rest  of  the  residual  terms  can  be  found  in  a  recursive 
manner  as  /=3,4,5,6  and  so  on.  Therefore,  a  general  form  of  residual 
terms  can  be  obtained  as 

W„  =  W„,A,  +  2W,.1QA,q,  +  W,.,QQA,q,q,  (A.20) 

where  i=l,2 6 
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APPENDIX  B.  DERIVATION  OF  DIRECT  DYNAMICS 

A  direct  dynamics  is  modeled  by  using  the  Lagrangian  method.    The 
Lagrangian  equation  is  given  as 


6_ 
dt 


f 


<?(KE) 


V 


*KE)  +  4?  =  P,    0=1,2 „)  (3.1) 


at,  )    d$j      dl 

where  £  are  the  generalized  coordinates,  n  is  the  number  of  degrees  of 
freedom  and  Py  denotes  the  generalized  forces. 

The  generalized  coordinates  are  defined  as 

where  the  generalized  coordinate  q,  for  large  motion  represents  the 

angular  position  of  link  /  and  the  generalized  coordinate  8i  for  small 

motion  represents  the  angular  displacement  between  q,  (i.e.,  link  position 

variable)  and  motor  position  variable  qmi. 

The  generalized  force  Py  obtained  from  the  concept  of  virtual  work  is 

defined  as 

i=l  i=l 

where  TG(  is  the  generalized  force  acting  on  the  generalized  coordinate 
q,.,  and  -T  is  the  generalized  force  acting  on  the  generalized  coordinate 

Two  dynamic  equations,  named  the  dynamic  equations  of  large  motion 
and  the  dynamic  equations  of  small  motion  according  to  generalized 
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coordinates,  are  derived  separately  in  general  forms  for  the  sequential 
integration  method  in  simulation. 

A.  THE  DYNAMIC  EQUATION  OF  LARGE  MOTION 

The  dynamic  equation  of  large  motion  is  derived  from  Lagrangian 
equation  with  generalized  coordinate  of  link.  Performing  the  differentiation 
of  Equation  (3.7)  with  respect  to  (^obtains 


o  q;  2  ,=; 


-J.W.  +W.J.  -T— - 


<H    '     ' 


(B.l) 


by  introducing  the  notation 


d  W       d  W 

__i  =  __i  =  W  . 


for  i  >  j 


(B.2) 


Equation  (B.l)  can  be   rewritten  as 


where 


d(KE)^=lytr(w  JWT  +W.J  w.T.) 


tr(WiXWT)  =  tr(WiJiWi;/) 


(B.3) 


(B.4) 


and  (B.3)  becomes 


^f^  =  ttr(W^WT) 


(B.5) 
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by  substituting  Equation  (B.4)  into  Equation  (B.3). 

Differentiate  Equation  (B.5)  with  respect  to  time  gives 


d_ 
dt 


<?(KE) 


LX 


=  Ytr(W.J.W.T  +W  JWT) 


(B.6) 


'=y 


since 


W,,=IW„Mqt 


(B.7) 


*=1 


Equation  (B.6)  becomes 


d_ 
dt 


d(KE) 


LK 


<H 


=  Itr 


J.W    +  W  JW 

II  L.J         I  1 


(B.8) 


IW,,,qt 

The  second  term  of  the  Lagrange  equation  due  to  link  kinetic  energy 
is  obtained  by  differentiating  Equation  (3.7)  with  respect  to  q/ 


where 


<?(KE) 


LK 


<H 


1    6 


-JW    +W..J 


<H  '  ' 


<H 


V^;y 


=  Itr 


r^w 


«=/ 


-JW 


<?q,  '  ' 


(B.9) 


tr 


<?W. 


-J.W. 


<H  '  ' 


=  tr 


WJ    — — 


and 


W=XW,,q, 


*=1 


(B.10) 
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<?w 


Equation  (B.9)  then  becomes 


L  =  W,„=IW.Mq1  for       i>j 


(B.ll) 


k=\ 


d(KE) 


LK    _ 


=  1" 


'lW,,,q, 


JW 


(B.12) 


Similarly,  performing  the  differentiation  of  Equation  (3.14)  with  respect 
to  q>  obtains 


d(KE)        «  1 


^W 


1-1 


A  J   Ar.W.  ,+W.  , 

mi     nu        mi         i-l  i-l 


dA. 


+W.  ,A   J  . 

i—l        mi     m< 


*4, 

r^A...Y.i.T 

) 


<H 


J   AT  W  , 

mi        mi         i-l 


<H 


W  , +W  ,A  J   AT 

i-l  i-l        mi     mi        mi 


f<?W,-i 


+2 


r)  W  r)  A 

-^A   J  .A^W'+W, ^J  .AT.WT, 

mi      mi        mi  i-l  i— 1        -»     •  mi        mi  i-l 


Hj 


<H 


+W  ,A  J 

i-l       mi     mi 


v 


H, 


W.  ,+W.  ,A  .J    A 

i-l  i-l        mi     mi        mi 


+ 


dw 


1-1 


dq, 


A   J   ATWT, +W  , 

mi     mi        mi         i— 1  i— 1 


dA 

mi 


J   AT  WT, 

mi        mi         i—l 


+W  ,A   J  . 

i-l        mi     mi 


d  A  "l 

mi 

~H 


WT, +W  ,A   J   A1 

i-l  i-l        mi     mi        mi 


<?w 


1-1 


<H 


(B.13) 


Since  Am.  are  not  functions  of  q,,  and  i>  j  +  1,  thus 


<?Am, 
Hj 


=  0 
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(dW 


=  o 


By  introducing  the  notation 


<?W 


.-I 


<H 


—  =  w 


»-!.> 


(B.14) 


Equation  (B.13)  becomes 


d(*CE)m  =  V  V(W  .  A   J   AT  W7,  +W  ,A   J    AT W\  ) 

~\    •  ^_^    ^n        I  \         i-l, y        mi     mi        mi         i-l  «-l        mi     mi        mi         i— 1 ,_/  / 

o  q .         ,=>+i  z    L 


+2 


W  .  A   J  .AVW'.+W.  ,A   J 

i— l, j        mi     mi        mi         i— 1  i— 1        mi     mi 


f*A_Y     , 


+ 


W 


dk 

mi 


■J-.-ALWiT.,+Wi,1AB1.JB1. 


(B.15) 


as 


and 


tr(W  ,  A   J   AT.WT,)  =  tr(W.  .A   J    ATW\  )  (B.16) 

\  i— I, j        mi      mi        mi  i— 1  J  \         i-l         mi      mi        mi  i  —  \,j  J 


tr  Ww  — ^J^.ALW^ 
I  <H  ) 


=  tr 


W.  ,A   J  . 

i-l        mi     mi 


(dk 


J^i     . 


w 


i-l 


(B.17) 


one  can  finally  obtain 
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<?(KE), 


=  itr[w,_,/Amjm,A:,w,:1  +w,.,,/a„.j„a:w,:1] 


i=y+l 


+Itr 


W  ,A  J 

«— 1       mi     mi 


f*0 


w  +w 


1-1 


dK 
*<j; 


J   A   W  , 

mi        mi         i-l 


(B.18) 


Introducing  the  notations  as  follows: 


dA. 


A  .  =  "  "  "q  .  =  A  .        where  i=j 

mi  -\  Tny  my,y 

Tmy 

q     =  Gq -8 


^^  mi 

w  [  <?  q, 


y 


0,  if/*./ 


(B.19) 
(B.20) 

(B.21) 


substituting   Equation  (B.21)   into  Equation   (B.18),  Equation   (B.18) 
becomes 

<?(KE), 


<H 


=  Ytrfw  ,  A   J   ATWT,  +W  ,  A   J  .A'.W*,1 

^_  ,_1./        mi      mi        mi  i-l  »-l./        Tru      mi        m'  ,_'  J 


i=y+l 


+trfW..A  J   G  AT    WT  +W.  .G.A     J   ATW\1(B.22) 

[         y-1        ny     ny        y        my.y  ;_,  /-l        y        my,y     my        my         y-1  J 

Differentiating  Equation  (B.22)  with  respect  to  time  obtains 


d_ 


=  tr(w.  ,A  .J.GA'.W'  +W,  .AJ.GX/W 

^         y-l        my     my        y        my.y  ,_,  y-1        my     my        y        my.y  H 


+W  ,A   J   G  A     W    +W,A   J   GA     W 

y-1        my     my        y        my.y  ;_,  y-1        my     my        y        my.y  ;_, 

+W.  ,G.A    J   AT  WT,+W.  ,GA    J  .A\W.\ 

y-1        y       my.y     my       my         y-1  y-1       y       my.y     my       my         y-1 

+W..G.A    J  AT.WT.+W.  .A  J.GA1..W' 

y-1        y        my,y     my        my         y-1  y-1        my     my        y        my.y  ;-_, 
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+ itr(w,.wAm,jmiA:,w,:1  +w,_,,Am,jmA:w.:1 

+W. .  .A  J  .AT.W.T,  +  W. .  .A  J  .AT.W.T, 

i— l,y       mi     mi       mi        i— 1  i— l,y       mi     mi       mi         i-l 

+W. ,  .A  J  .AT.WT,  +W. .  .A   J  .A'.W.7, 

i-\, J       mi     mi       mi        i-l  '-I./       mi     mi       mi         i-l 

+W.  .  .A  J  AlW*  +W,.A  J   AT.WT,)  (B.23) 

i—l, y       mi     mi       mi        i-l  *— 1./       mi     mi       mi         i-l  /  x  ' 


where 


trfw.  ,A   J   G  AT   WT  )  =  tr(\V .  ,A  J   G  AT   WT  ) 

^         y-l       my     ny       y       my.y         H  J  \        ;-l       my     my       y       my.y         H  ) 

Equation  (B.23)  then  becomes 


d_ 
dt 


<?(KE), 


=  tr(\V  ,A   J   GAT..WT  +  2W.  ,A   J   G.A\.WT 

y         y-l        my     my        y        my.y  H  y-1        my     my        y        my.y  H 


+W  .  A   J   AT  WT,  +  W.  .  .A   J  .AT.W.T, 

i-l,y       mi     mi       mi         i-l  '-'./       m'     m<       m'        '-1 

+W.  ,GA  ..J  .AT.W.\+W.  ,G  A    J    AT.WT, 

y-l        y        my.y     my        my         y-l  y-l        y        my.y     my        my         y-l 

+W.  ,.A   J   AT  WT,) 

i-l,y        mi     mi        mi         i-l  / 

+  Y tr(W  ,  A   J  .AT.WT,  +W  .  A   J  Al.W* 

X-j        \         <-l,y        mi     mi        mi         i-l  j— 1,/        mi     mi        mi         i-l 

i=y+l 

+2W.  ,  A   J  .AT.W.T,  +  W.  .  .A   J  .AT.WT, 

i-l, y        mi     mi        mi         i-l  '-'./        mi     mi        mi         i-l 

+W  .  A   J  .AT.W.\  +  W  .  A   J   AT  WT, 

i-l,y        mi     mi        mi         i-l  '~i.J       mi     mi        mi         i-l 

+W  ,.A   J    AT  W.T,)  (B.24) 

i-l,y       mi     mi       mi         i-l  / 

The  second  term  of  Equation  (3.1)  due  to  motor  kinetic  energy  is 


<?(KE)        «1 
<?  q,         fiT  2 


r)  W  r)  A 

-^-A  J   A^W.^+W.  . -J   ATWT, 

mi     mi        mi         i— 1  i— 1       ~i    _  mi        mi         i— 1 


+W  .A   J  . 

i-l        mi     mi 


<H 


WT,  +W .  ,A  J   AT 

i-l  i-l        mi     mi        mi 


(dVi  ,"1 

i-i 
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+2 


—A  J  .AT.W71+W.1-r-J=J  AT.WT, 

nu     mi        nu         i-l  i-l       ~i  mi        mi         i-l 


L*«j 


<H 


dA 


/ 


+W4_1Anu.Jmj— -iW^  +  W,_1AmiJmiA 


g  WM  ' 


+ 


(5W  ,9  a 

-^A  J  AT.WT.+W.  .— —  J  .AT.WT, 

mi     mi        mi         i— 1  »-l       "|  mi        mi         i-l 


L<H 


+w,_,A„,j, 


(dk.) 

m« 


<H 


W,T_,  +  W,_,  A^AL 


( 


dW.A 


<H 


i-l 


(B.25) 


As  the  terms  — and   — exist  only  when  /  >  7  + 1  the  terms 

d  q .  d  q . 


d  A_  dk 

•and  -  exist  only  when  i>  j,  and 


<?q,        <?q, 


— ^A  J  .AT.WT, 

mi     mi        mi         i-l 


V 


<H 


=  tr 


J 


W  ,A   J  AT. 

i-l        mi     mi        mi 


fdH.    V 


i-l 


<H 


tr 


(     ■  ^  Am,  T       •     T    ^ 

W.  ,-— ^J    ATWT 


i-i 


V 


*q. 


mi        mi         i-l 


=  tr 


) 


W  ,A  J  . 

i-l        mi     mi 


Oa„Y.t^ 
J       ) 


*% 


f^W,l  -T  T 


=  tr 


W.  ,A  J  AT. 

i-l        mi     mi        mi 
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( 


tr 


W 


V 


dk 

m 


J  .ATWT, 

mi        mi  i—l 


=  tr 


W.  ,A  J 

i-l        nu     mi 


dk 

mi 

^7 


v    ^ 


w 


1-1 


Equation  (B.25)  can  be  rewritten  as 


d  (KE)„      iit 

d  q;         ,=y+i  2 


2-^-^-A_..J_..Al.WT, +2-r-^A   J   ATWT 


<H 


mi     mi        mi         i-l 


<H 


+2W  .A  J   AT 

i-l        mi     mi        mi 


^w,v    <?w 


i-l 


V 


+  2- 


.-i 


mi     mi        mi         i-l 


T   1I7T 


d%       J  d% 


A  J    A'.W., 

mi     mi        mi         i-l 


1 

+  -tr 
2 


2W .,^-2- J    AT.WT,+2W.,^^J    ATWT 

ny        my         y-l  y-l 


M 


+2W  ,A   J  . 

y— 1        my      my 


(dk 


<H 


*q. 


my        my  j—\ 


WT,+2W  . 

y-l  y-1 


dk 


**> 


^J    AT  WT, 

my        my         y-l 


(B.26) 


Introducing  the  notation 


dk 


"V    _ 


<H 


=  A 


m;.y 


(B.27) 


Equation  (B.26)  becomes 


*(KE), 


=  f  tr(W.  ,  A   J   A'  W.',  +W  ,  A  J  A'.W.', 

jL^        \         t — 1  ,y        mi     mi        mi         i— 1  i—  l,y        mi     mi        mi         i— 1 


i=y  +  l 


+w,_,Amjm,  Aiw,:,, + w,.,/Anvjm,A:,w,:1 ) 

+tr(W/_1G;Am/JJ„vA;,W/_1  +  *H°/V.  A>/-> 

+w;_, a^g, a  ;x,  +  w,.,g,a;,,j„,a;w;,  )  o ^ 


45 


The  last  term  of  Equation  (3.1)  is  obtained  by  differentiating  Equation 
(3.20) 

d  (PE)         6  6 

-^  =  -£m,r;  W,:g-mXTA;,w;,g-  X-n^AXj  (B.29) 
a  qy  .=>  .=>+! 


X  =W  ,A    +W.,A  . 

X   =W  ,A  . +2W.  ,A   W  ,A  . 

y  y-1        my  y-1        ny         y-1        my 


Introducing  the  notations 

(B.30) 

(B.31) 

(B.32) 

and  combining  Equations  (B.8),  (B.12),  (B.24),  (B.28)  and  (B.29) 
according  to  Equation  (3.1)  gives  the  final  general  form  of  dynamic 
equation  of  large  motion,  i.e., 

Ytr(W  J  WT)  +  tr(X  J    AT    W\)+  Ytrfx  J    A?  W'    ) 

A-l       \         «./     »         «     /  \        y     my        my.y         y-1  /  £^        \        y     my        my         <-l,y  / 


»=y 


i=y+l 


-Ymr'  W  g-m  r'A     W  ,g-  Ym   r  A   W .  .  g  =  G  T 

Xm*  I     I  'i/°  mJ     "V  "V-y  J  — Its  4^  mi     mi         mi  i-],yO  y       y 


«=; 


i=y+l 


(B.33) 


B.  THE  DYNAMIC  EQUATION  OF  SMALL  MOTION 

The  dynamic  equation  of  small  motion  is  derived  from  Lagrangian 
equation  with  generalized  coordinate  of  deflection.  The  first  term  in 
Equation(3.1)  can  be  written  as 


where 


d  f<?(KE) 


dt 


\ 


( 
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j     J 


dt 


\        A    ( 


[  d  (KE)^  1     d 


V 


dS 


J     J 


dt 


d  (KE), 


V 


dS 


j     J 
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d 
~dt 


<?(KE) 
98 


LK 


=  0 


i       J 


therefore,  performing  the  differentiation  of  Equation  (3.14)  with  respect  to 
8 .  gives 


g  (KE), 
9  8, 


v1 

i=l  Z 


(3W  /9  A 

-2-A  J  A'.W'+W.  . ^J  AT.WT, 

mi      nu        mi         i— 1  i-l        -»     c*  mi        mi  i-l 


<?5 


+W.  ,A  J  . 

i—l        mi     m< 


"  9  8 


WT, +W  ,A   J   AT 

i-l  i-l        mi      mi        mi 


r9\v 


I-l 


V<H    ) 


+2 


&     »»  :    i  •    t  t  C    ** _ .  *    T  -I- 

—A  J  .AT.WT.+W.  . ^J  AT.WT. 

mi     mi         mi         i— 1  i— 1       -\    r*  mi        mi         i— 1 


98 


98 


+W  .A  J  . 

i-l        mi     mi 


(dk    V 


y38,  j 


W7.,+W,.1A„JmAl, 


f9Y/ 


i-l 


v<H  , 


+ 


d  W.  .  -xt  ^  Am 

^-A   J  AT.WT,+W  , ^J    ATWT, 

~)    o  mi     mi        mi         i-l  i-l       ~)    C  mi        mi         i-l 


+W,_,A„J„ 


'9k    ? 


K**,J 


WT, +W.  ,A  J   AT 

i-l  i-l        mi     mi        mi 


Ow  ,v 


K**>   J 


(B.34) 


As  the  terms 


d  W ,     <?  W 


i-i 


9  8;       9  8, 


r^and 


9X 

n 


are  not  functions  of  8.  thus 


!•> 


9\V 


i-i 


9    8: 


=  0 
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<?w 


d5: 

dA 

n 


^-  =  0 


=  o 


and 


tr 


W.  .A   J  . 

i-l        mi     mi 


W 


i-l 


=  tr 


W.. : 

v        9*> 


i"    T       A  T  \xtT 


J   A1  W1, 

mi        mi         i-l 


where         •     exists  only  when  i>j,  Equation  (B.34)  then  becomes 


d  (KE)M  =  1 
(95        "2F 


^  i    V 


2W.  ,A   J  . 

j— 1        my     ny 


<9  A. 


my 


v<H  , 


W;_,T+2W,_, 


<dk    > 


my 


V^W 


Tti;       T 


J    A     W  . 

my        my  y-1 


(B.35) 


introducing  the  notation 


9  A 

— !a-  =  -A  .. 


(B.36) 


Equation  (B.35)  becomes 


*  (KEX 


=  tr(-W.  .A   J   A  .  .W  ,T-W.  .A    J    A   TW  ,T)(B.37) 

\  j-\       my     my       my.y         y-1  y-1       my.y     my       my  y-1     / 


Differentiating  Equation  (B.37)  with  respect  to  time  obtains 
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d 

It 


f 


J  (KB), 

d8 


\ 


=  tr(-W.  .A   J  .AT..WT,-W.  ,A  J   A\  W.\ 

V  y-1        ny     my        mj,j         j-l  ]-\        my     ny        my.y         j-\ 


■W.  .A   J  .AT..WT   -W.  .A   J  .AT..W.\ 

y-1       ny     ny       ny,y        y-1  j-\       ny     ny       my.y        j-\ 


-w,,,a„,,.j„a;w;,  -  wy„ a^j^a;w;_, 

-W^A^AVw:,  -  W;, A^JwA;w;, )      (B.38) 
The  second  term  of  Equation  (3.1)  is  obtained  as 


d  (KE)        1 

— ^  =  -tr 

d8  2 


d  A  T      T  .       d  A 

2W, ,  -r-J*-  J..AI.W ,  +2W, ,  -r-r*-  J    AW 


;-i 


<?<5. 


i-i 


<?<5, 


/.w.\ 

my        my  y-1 


+2W,  ,A  .J  . 

y — l        ny     ny 


wh+2V„ 


VJ   A  .W ' 

my        my  y— 1 


and  as 


(B.39) 


dA  d  A 

ny_  _  _  A  — HL 


(B.40) 


—  (-A  .  )  =  -A  ..= ^ 

dt\  ny.y/  my.y  ^  £ 


(B.41) 


Equation  (B.39)  can  be  rewritten  as 

^f^  =  tr[-W,_,Am,.,JwA;,W/-,  -*„\J*K*» 

-W,_,Am,Jm,Am,./ w;,  -  W;_IAm;,/JTOA:jW;_,  ]         (B.42) 

The  last  term  of  Equation  (3.1)  is  obtained  by  differentiating  Equation 
(3.20)  with  respect  to  5 .: 
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Since 


<9(PE) 
9  8; 


=-m^(-A;>)w;1g+k^ 


(B.43) 


dt 


<9(KE) 


LK 


dS 


=  0 


<?(KE) 


LK 


<?<5; 


/     / 


=  0 


<?(PE) 


LK 


dd 


=  0 


combining    Equations  (B.38),  (B.42)  and  (B.43)  according  to  Equation 
(3.1)  gives  the  final  general  form  of  dynamic  equation  of  small  motion,  i,e., 


■tr(X  J   AT    WT,)  +  m   r;TAT    W\g  +  k  <5  =-T  (B.44) 

\        /     ny        mj.j         i-l  /  ny    ny         ny,/         j-\0  j      j  j  \  ' 


where 


X   =W    A 

j  ]-\        ny 


X   =W    A    +W,A  . 

J  j-\        ny  j-\        ny 


X   =W  .A  . +2W.  .A   W  .A  . 

;  j-\        ny  j-\        ny         j-\        ny 


(B.45) 
(B.46) 
(B.47) 
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APPENDIX  C.   NEWTON-EULER  RECURSIVE 
FORMULATION 

Referring  back  to  the  definition  of  the  absolute  angualr  velocity  of  link 
i  for  revolute  joint: 

®i=R,m(«h+»m)  (CI) 

By  premultiplying  rotation  transformation  matrices,  the  angular  velocity  of 
link  i  is  transformed  as 

R>,=R;-'(a),_1+«,,)  (C.2) 

Differentiating  Equation  (C.2)  w.r.t.  time  according  to  R'0  =  R'on  yields 

R'0Q^,  +R'0(bt  =R;-1Qi_1(wi_1  +  a>M)  +  Rj*(<frH  +  <frM)  (C.3) 

since 

equation  (C.3)  becomes 

R>,  =RJ,-'Ql.1(fi,i.1  +ffllj)  +  R;-'(ri)w  +«,,)  (C.4) 

multiplying  Equation  (C.4)  by  R°  in  both  sides  gives 

<u,=R;-,(a,_1<»,,  +  ft>,.,+tt>,,)  (c.5) 

Where 


r;r; -  =  r;-  and  r»r;  = 


"1 

0 

0" 

0 

1 

0 

0 

0 

1 

(C.6) 


Similarly,  differentiating  Equation  (C.6)  once  and  twice  w.r.t  time  gives 
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co,  =  K1  [ow  (GM*»g  +  *>,-, )  +  0H«U  +  20Mfl>u  +  *H  +  K  ]  -  atcot 

(C.7) 
">, ,  =R;-,(Q?_1fl)l,  +QmQwi»h  +  2QHfiHa>u  +  3G,_A-Ai 
+  2Q,._16>,_1  +3QHaM  +OhQm©m  +3QJ_1<  +flH©M 

+  flH  +®u)-(QiQA +2°A)  (C.8) 

The  linear  acceleration  of  link  i  expressed  in  terms  of  base  coordinate 
is  given  as 

R0a,  =Ri(RlX-,  +"AS1,  +  6,81,)  (C.9) 

differentiating  Equation  (C.9)  w.r.t.  time  yields 

RiQ^+RjA,  =Rj-1(QHaH  +ai,)  +  R'0(Q(QA+2QA  +  QA  +  A)SI, 

(CIO) 
By  multiplying  R°in  both  sides,  Equation  (CIO)  can  be  rewritten  in  a 

recursive  form  of  derivative  of  the  linear  acceleration,  i.e., 

a  =R'1(Q  ,a  ,  +  a  ,)  +  (Q3  +2Q.Q  +CIQ  +Q.  )S\  -Qa       (CM) 

i  i       V       j— 1      i—l  i-l  /         \        i  ii  ii  ill  ii  v  ' 

Similarly,  differentiating  Equation  (C.ll)  gives  the  second  derivative  of 
linear  acceleration,  i.e., 

a  =R,1(Q2,a  .  +  2Q  ,a  ,+Q  ,a  .  +a  .) 

■        i   ^    i-i  i-i  j-i  i—i        i-i  i-i       i-i  j 

+  (Q4  +  3Q2Q  +QQQ  +  2Q.Q.Q.  +3Q2  +3Q.Q  +  QQ  +Q.  )S1 

y    i  ii        iii  iii  i  ii        ii        .  / 

-(ftfa,  +  Q.aj+2Qa)  (CI  2) 

The  derivatives  of  force  and  torque  can  be  obtained  by  following  the 
same  way.  The  results  are  as  follows: 

f.  =F+R;+1f+1  (CI  3) 
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f,  =  Q,F  +  F,  +  Rf  (Q,JM  + 1, )  -  QA  (C 1 4) 

f,  =£2AF,  +  2*2,$  +Q,F  +F,  +  R;«(0MQwrw  +  20.X  +  6„f„  +FM) 
-(OQf  +  2Qf  +  Qf) 

\       i       i    i  i    i  i    i  / 

Fi  =  m,aa 

F,  =  m^ 

a^a.+QCU^A 
a    =a  +  Q  Cl  +  Q.Q.C1.  +Q.Q.C1. 

Cl  I  I  I  111  III 

a  .  =a  +Q  Cl  +  Q.Q.CI.  +2Q.Q.CI.  +  Q  Q  Cl 

CI  I  II  III  III  III 

N  =16)  +Q\  co 

■       ii        iii 

N,.  =  1.^.  +6.1,(0.  +Qlicbt 

N  =lcb  +QI  G)  +2QI.tt)  +QI  a) 

i       ii        iii  iii        iii 

n .  =N  +R,+1n  ,+SI  xf  +C1  xF 

i        i        i     i+i        it         ii 

n.  =  Q.N, i+  % +  R;+1  (Q, +1ni+1  +  ri, :+1)  +  Q, (SI,  xf^  +  Sl,  xf +a(Cl,  x  F) 

+Cl,xF-Q(n(  (C.26) 

ii.  =a.Q.N.+2Q.N.+Q.N.  +  N.+Ri+1(a.  ,ft+1n+1  +  2Q+1ri+1 

i      iii       it      ii      i      i   \   i+l   i+l  i+l        i+l   i+l 

+Q+1n+1  +n.+1)  +  Q.Q  (SI  xf  )  +  2Q.  (SI  xf  )  +  Q  (SI  xf  ) 

i+i     i+i  i+i  j  ii\i         i/  <  y      j         i  j  i\i         '/ 

+SIixf(+QiQi(ClixFj  +  2a(Cl(xF)  +  ni(ClixF)  +  ClixF 
-Q.O.n  -2ftri  -Qn 


(C.15) 

(C.16) 
(C.17) 
(C.18) 
(C.19) 
(C.20) 
(C.21) 
(C.22) 
(C.23) 
(C.24) 
(C.25) 


■    i   i 


(C.27) 
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